#include "src/graph.h"

namespace pathfind {

Graph::Graph(std::unordered_map<IntPoint, int> &map, int width, int height)
    : map_(map), width_(width), height_(height) {}

float Graph::cost(const IntPoint &from, const IntPoint &to,
                  const IntPoint &goal) const {
  if (from.x == to.x || from.y == to.y)
    return 1.0f;

  return 1.414f;
}

float Graph::heuristic(const IntPoint &from, const IntPoint &to,
                       const IntPoint &goal) const {
  int dx = std::abs(goal.x - to.x);
  int dy = std::abs(goal.y - to.y);

  return static_cast<float>((dx + dy) - 0.586 * std::min(dx, dy));
}

std::unordered_map<IntPoint, float> Graph::get_neighbours(const IntPoint &p) {
  neighbours_.clear();

  for (int i = -1; i <= 1; i++) {
    for (int j = -1; j <= 1; j++) {
      if (i == 0 && j == 0)
        continue;

      IntPoint neighbour(p.x + i, p.y + j);
      if (is_position_valid(neighbour)) {
        add_neighbour(neighbour);
      }
    }
  }

  return neighbours_;
}

DIRECTION Graph::get_direction(IntPoint &from, IntPoint &to) const {
  if (from == to) {
    return DIRECTION::STILL;
  } else {
    if (to.x == from.x) {
      return to.y > from.y ? DIRECTION::TOP : DIRECTION::BOTTOM;
    } else if (to.y == from.y) {
      return to.x > from.x ? DIRECTION::LEFT : DIRECTION::RIGHT;
    } else if (to.x > from.x && to.y > from.y) {
      return DIRECTION::TOPLEFT;
    } else if (to.x > from.x && to.y < from.y) {
      return DIRECTION::BOTTOMLEFT;
    } else if (to.x < from.x && to.y > from.y) {
      return DIRECTION::TOPRIGHT;
    } else if (to.x < from.x && to.y < from.y) {
      return DIRECTION::BOTTOMRIGHT;
    } else {
      return DIRECTION::STILL;
    }
  }
}

bool Graph::is_position_out_of_range(const IntPoint &p) const {
  return p.x < 0 || p.x >= width_ || p.y < 0 || p.y >= height_;
}

bool Graph::is_position_valid(const IntPoint &p) const {
  return map_.find(p) == map_.end() ? false : true;
}

int32_t Graph::get_weight(const IntPoint &p) const {
  if (map_.find(p) == map_.end()) {
    return -1;
  } else {
    return map_.at(p);
  }
}

void Graph::add_neighbour(const IntPoint &p) {
  if (neighbours_.find(p) == neighbours_.end()) {
    const_map_iterator_ = map_.find(p);
    if (const_map_iterator_ != map_.end() && const_map_iterator_->second != 0) {
      neighbours_.insert(
          std::make_pair(p, static_cast<float>(const_map_iterator_->second)));
    }
  }
}
}
